/*****************************************************************************
 * $LastChangedDate: 2013-05-26 14:26:47 -0400 (Sun, 26 May 2013) $
 * @file
 * @author  Jim E. Brooks  http://www.palomino3d.org
 * @brief   Math functions (vector).  See math/funcs.hh.
 * REMINDERS: - Consider using MATH_EXCEPTION() instead of ASSERT().
 *//*
 * LEGAL:   COPYRIGHT (C) 2004 JIM E. BROOKS
 *          THIS SOURCE CODE IS RELEASED UNDER THE TERMS
 *          OF THE GNU GENERAL PUBLIC LICENSE VERSION 2 (GPL 2).
 *****************************************************************************/

#ifndef MATH_FUNCS_VECTOR_HH
#define MATH_FUNCS_VECTOR_HH 1

#include <utility>
#include <cerrno>
#include "base/clamp.hh"
#include "math/types_trig.hh"
#include "math/defs_vector.hh"
#include "math/vertex.hh"
#include "math/funcs_debug.hh"
#include "math/funcs.hh"
#include "math/funcs_trig.hh"

namespace math {

////////////////////////////////////////////////////////////////////////////////
//////////////////////////////  V E C T O R   //////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Distance between vector and implied zero origin.
 *****************************************************************************/
template<typename FP>
FP
Distance( const FP x, const FP y, const FP z )
{
    return SquareRoot( (x*x) + (y*y) + (z*z) );
}

INLINE fp
Distance( const Vector2& v )
{
    return SquareRoot( v[XX] * v[XX]
                     + v[YY] * v[YY] );
}

INLINE fp
Distance( const Vector3& v )
{
    return SquareRoot( v[XX] * v[XX]
                     + v[YY] * v[YY]
                     + v[ZZ] * v[ZZ] );
}

/*****************************************************************************
 * Distance between two vectors (DistanceSquared() is faster than this).
 *****************************************************************************/
template<typename VECTOR3A,typename VECTOR3B>
fpx
Distance( const VECTOR3A& v1, const VECTOR3B& v2 )
{
    const fpx dx = v2[XX] - v1[XX];  // vector types may differ,
    const fpx dy = v2[YY] - v1[YY];  // use extended-precision
    const fpx dz = v2[ZZ] - v1[ZZ];
    return SquareRoot( dx*dx + dy*dy + dz*dz );
}

/*****************************************************************************
 * Calculate the distance^2 (squared) between two vectors.
 * DISTANCE RETURNED IS SQUARED (distance^2).
 * SQUARE ROOT OF DISTANCE IS SKIPPED FOR SPEED.
 * sqrt() and the underlying FPU instruction is very slow.
 *****************************************************************************/
INLINE Vector3::value_type
DistanceSquared( const Vector3& v1, const Vector3& v2 )
{
    Vector3::value_type dx = v2[XX] - v1[XX];
    Vector3::value_type dy = v2[YY] - v1[YY];
    Vector3::value_type dz = v2[ZZ] - v1[ZZ];
    return dx*dx + dy*dy + dz*dz;
}

/*****************************************************************************
 * @return A normalized vector (unit-length 1.0) of the same vector.
 * Not to be confused with calculating the normal vector of two vectors (CrossProduct).
 *****************************************************************************/
INLINE Vector3
Normalize( const Vector3& v )
{
    const Vector3::value_type d = Distance( v );
    const Vector3::value_type x = v[XX] / d;
    const Vector3::value_type y = v[YY] / d;
    const Vector3::value_type z = v[ZZ] / d;
MATH_EXCEPTION( (not IfNan(x)) and (not IfNan(y)) and (not IfNan(z)),
                "math error: Normalize(Vector3) NaN" );
    return Vector3( x, y, z );
}

template<typename VERTEX,typename FP>
VERTEX
Normalize( const FP x, const FP y, const FP z )
{
    const typename VERTEX::value_type d = Distance( x, y, z );
    const FP nx = x / d;
    const FP ny = y / d;
    const FP nz = z / d;
MATH_EXCEPTION( (not IfNan(nx)) and (not IfNan(ny)) and (not IfNan(nz)),
                "math error: Normalize(Vector3) NaN" );
    return VERTEX( nx, ny, nz );
}

/*****************************************************************************
 * Normalize a 3D point (unit 1 distance) then return one normalized coordinate.
 * The return value typically is passed to asin().
 *****************************************************************************/
INLINE Vector3::value_type
NormalizeCoord( const uint axis, const Vector3& v )
{
ASSERT_AXIS3(axis);
    Vector3::value_type d = Distance( v );
    Vector3::value_type n;
         if ( axis == XX ) n = v[XX] / d;
    else if ( axis == YY ) n = v[YY] / d;
    else   /* axis == Z */ n = v[ZZ] / d;
MATH_EXCEPTION( not IfNan(n), "math error: NormalizeCoord() NaN" );
    return n;
}

/*****************************************************************************
 * Increase/decrease a 3D vector by a scalar.
 *
 * The WRONG way, of course, is to simply add the scalar to each dimension.
 * For example, Vector(1,1) vs. Vector(2,2):
 * math.sqrt( (1.0*1.0) + (1.0*1.0) )
 * 1.4142135623730951
 * math.sqrt( (2.0*2.0) + (2.0*2.0) )
 * 2.8284271247461903  # oops != 2.4142
 *
 * The right way is to turn the scalar distance into a 3D vector
 * with the same direction as the input vector.
 * This is done by multiplying the scalar distance
 * by the normalized input vector, then adding.
 *****************************************************************************/
INLINE Vector3
AddDistance( const Vector3& v, const fp distance1 )
{
    const Vector3 norm3 = Normalize( v );
    const Vector3 distance3( distance1 * norm3[XX],
                             distance1 * norm3[YY],
                             distance1 * norm3[ZZ] );
    return Vector3( v[XX] + distance3[XX],
                    v[YY] + distance3[YY],
                    v[ZZ] + distance3[ZZ] );
}

/*****************************************************************************
 * Calculate a cross product of two vectors which yields a normal vector
 * that is perpendicular to the plane containing the two source vectors.
 * Note: The cross product won't be unit length.
 *****************************************************************************/
INLINE Vector3
CrossProduct( const Vector3& v1,
              const Vector3& v2,
              const Vector3& v0 = Vector3(0,0,0) ) // origin of v1 and v2
{
    Vector3::value_type xd1 = v1[XX] - v0[XX];
    Vector3::value_type yd1 = v1[YY] - v0[YY];
    Vector3::value_type zd1 = v1[ZZ] - v0[ZZ];
    Vector3::value_type xd2 = v2[XX] - v0[XX];
    Vector3::value_type yd2 = v2[YY] - v0[YY];
    Vector3::value_type zd2 = v2[ZZ] - v0[ZZ];
    return Vector3( yd1 * zd2 - yd2 * zd1,    // x: y*z
                    zd1 * xd2 - zd2 * xd1,    // y: z*x
                    xd1 * yd2 - xd2 * yd1 );  // z: x*y
}

/*****************************************************************************
 * Calculate the dot product of two vectors which yields a scalar.
 * If the angle between the two vectors is between 0 and 90 degrees,
 * the dot product will be positive, else negative.
 *****************************************************************************/
INLINE fp
DotProduct2( const Vector2& v1, const Vector2& v2 )
{
    return v1[XX] * v2[XX]
         + v1[YY] * v2[YY];
}

template<typename VECTOR3A,typename VECTOR3B>
fp
DotProduct3( const VECTOR3A& v1, const VECTOR3B& v2 )
{
    return v1[XX] * v2[XX]
         + v1[YY] * v2[YY]
         + v1[ZZ] * v2[ZZ];
}

// Extended-precision.
template<typename VECTOR3A,typename VECTOR3B>
fpx
DotProduct3_fpx( const VECTOR3A& v1, const VECTOR3B& v2 )
{
    return fpx(v1[XX]) * fpx(v2[XX])
         + fpx(v1[YY]) * fpx(v2[YY])
         + fpx(v1[ZZ]) * fpx(v2[ZZ]);
}

/*****************************************************************************
 * Compute angle (in radians) between two vectors sharing the same origin.
 * Unlike a vector, this computed angle has no orientation nor winding direction.
 * The angle should be always positive and never greater than 180 degrees.
 * The angle will be the same if one of the vectors is turned in the opposite direction.
 *****************************************************************************/
INLINE Radian
Angle2( const Vector2& u, const Vector2& v )  // Angle2/3() differ in DotProduct2/3()
{
    // Dot product formula indirectly provides the angle.
    // angle = arccos( DotProduct(u,v) / Distance(u) * Distance(v) )
    Vector2::value_type uv = Distance(u) * Distance(v);
    if ( ABS(uv) < 0.1f ) return 0.0f;  // prevent NaN and div by 0
    Vector2::value_type theta = DotProduct2(u,v) / uv;
    theta = Clamp( theta, -1.0f, 1.0f ); // acos() won't tolerate 1.00000001
errno = 0;
    Vector2::value_type rad = std::acos( theta );
MATH_EXCEPTION( errno == 0, "math error: Angle2() acos()" );  // if acos() failed
    return rad;
}

template<typename VECTOR>
Radian
Angle3( const VECTOR& u, const VECTOR& v )
{
    // Dot product formula indirectly provides the angle.
    // angle = arccos( DotProduct(u,v) / Distance(u) * Distance(v) )
    typename VECTOR::value_type uv = Distance(u) * Distance(v);
    if ( ABS(uv) < 0.1f ) return 0.0f;  // prevent NaN and div by 0
    typename VECTOR::value_type theta = DotProduct3(u,v) / uv;
    theta = Clamp( theta, -1.0f, 1.0f ); // acos() won't tolerate 1.00000001
errno = 0;
    typename VECTOR::value_type rad = std::acos( theta );
MATH_EXCEPTION( errno == 0, "math error: Angle3() acos()" );  // if acos() failed
    return rad;
}

/*****************************************************************************
 * Interpolate between two vectors.
 * @param   v1, v2
 * @param   fraction
 *          Fraction in range {0,..,1.0}.
 *          If fraction is 0.0, v1 is returned.
 *          If fraction is 1.0, v2 is returned.
 *          If fraction is 0.5, midpoint is returned.
 * @return Interpolated 3D point.
 *****************************************************************************/
INLINE Vector3
Interpolate( const Vector3& v1, const Vector3& v2, const fp fraction )
{
MATH_EXCEPTION( FP_GE(fraction, 0.0f) and FP_LE(fraction, 1.0f), "math error: Interpolate()" );
    return Vector3( v1 + ((v2-v1) * fraction) );
}

/*****************************************************************************
 * Midpoint.
 *****************************************************************************/
template<typename T>
T
Midpoint( const T& x1, const T& x2 )  // could be a vector type
{
//  return x1 + (x2 - x1) / 2;
    return (x1 + x2) / 2;  // faster
}

INLINE Vector3
Midpoint( const Vector3& v1, const Vector3& v2 )
{
    return Vector3( Midpoint(v1[XX], v2[XX]),
                    Midpoint(v1[YY], v2[YY]),
                    Midpoint(v1[ZZ], v2[ZZ]) );
}

/*****************************************************************************
 * Cycle {X,Y,Z}.
 *****************************************************************************/
INLINE uint
CycleAxis3( const uint axis )
{
    return (axis + 1) % 3;
}

/*****************************************************************************
 * Rotate a 3D vector around an axis.
 *****************************************************************************/
template<typename VECTOR>
VECTOR
RotateVector3( const VECTOR& v, const uint axis, const Radian rad )
{
    // If angle is 0 then sin(0),cos(0) caused multiplying by 0,1, resp.
    // The rotation pattern is:
    // c - s
    // s + c
    const std::pair<fp,fp> s_c = SinCos<fp>( rad );
    const fp& s = s_c.first;
    const fp& c = s_c.second;
    switch ( axis )
    {
        case XX:
        {
            return VECTOR( v[XX],
                           v[YY] * c - v[ZZ] * s,
                           v[YY] * s + v[ZZ] * c );
        }
        break;

        case YY:
        {
            return VECTOR( v[XX] * c - v[ZZ] * s,
                           v[YY],
                           v[XX] * s + v[ZZ] * c );
        }
        break;

        case ZZ:
        {
            return VECTOR( v[XX] * c - v[YY] * s,
                           v[XX] * s + v[YY] * c,
                           v[ZZ] );
        }
        break;

        default: ASSERT(false); return VECTOR(); break;
    }
}

} // namespace math

#endif // MATH_FUNCS_VECTOR_HH
